"""
几何形状物理工具
"""
from utils import *
import warnings
import args
# from . import args
from math import sqrt

class ShapeBase(np.ndarray):
    """
    多边形
    # 计算包围盒的几何属性
    # 存在的问题:
        - position不应该是自动计算的, 应该改为指定物体当前的position
        - 物体当前各个顶点坐标应该通过 [position + 原始形状 + rotation] 来确定, 即[平移+旋转].
    """

    def __new__(cls, a, position=None):
        obj = np.asarray(a).view(cls)
        if position:
            position = to_np(position)
        obj._position = position
        return obj

    @property
    def position(self):
        """
        中心点坐标
        """

        if isinstance(self._position, np.ndarray):
            return self._position

        xy1, xy2 = self.xy1, self.xy2
        ret = np.array([np.mean([xy1[0], xy2[0]]), np.mean([xy1[1], xy2[1]])])
        return ret

    @property
    def width(self):
        ret = self.xy2[0] - self.xy1[0]
        return ret

    @property
    def height(self):
        ret = self.xy2[1] - self.xy1[1]
        return ret

    @property
    def xy1(self):
        """
        左下角坐标
        """
        ret = self.min(0)
        return ret

    @property
    def xy2(self):
        """
        右上角坐标
        """
        ret = self.max(0)
        return ret

    def support(self, d):
        """
        返回d方向最远的点
        """
        ret = getFarthestPointInDirection(self, d)
        return ret


class Simplex:
    """
    二维单纯形: 三角形
    """
    def __init__(self, abc=None):
        """
        二维单纯形, 根据三角形的三个点初始化

        :param abc: list或np, 一个有三个点的矩阵.
        """
        if abc is None:
            self.ls = []
        else:
            self.ls = list(abc)

        self.d = None

    @property
    def a(self):
        if self.ls.__len__() < 3:
            return self.ls[0]

        return self.ls[-3]

    @property
    def b(self):
        if self.ls.__len__() < 3:
            if self.ls.__len__() < 2:
                return None
            return self.ls[1]

        return self.ls[-2]

    @property
    def c(self):
        if self.ls.__len__() < 3:
            return None

        return self.ls[-1]

    def add(self, data, rm_name='a'):
        """
        永远只能有3个点, 增加一个的话就得去掉一个, 默认去掉最前面的'a'

        ps: 其实用队列比较好, 先入先出. 用list逻辑会复杂点.
        """
        if rm_name and self.ls.__len__() >= 3:
            self.remove_by_name(rm_name)
        self.ls.insert(0, data)     # 先入后出, 栈

    def remove_by_name(self, p: str):
        if p == 'a':
            ret = self.ls.pop(0)
        elif p == 'b':
            ret = self.ls.pop(1)
        elif p == 'c':
            ret = self.ls.pop()
        else:
            ret = None
        return ret

    def remove_by_id(self, index=-1):
        return self.ls.pop(index)

    def info(self):
        show_ls(self.ls)

    def __repr__(self):
        return f'共{len(self.ls)}个, 元素:{self.ls}'

    def __len__(self):
        return len(self.ls)

    def ContainsOrigin(self):
        """
        根据三角形三个点的坐标, 判断其是否包含原点
        即判断ao是否在ac和ab之间(由退出条件可知, 原点已经在bc内侧方向)

        :param a: 点a
        :param b: 点b
        :param c: 点c
        :return: tuple(ret, d, simp), ret: 是否碰撞, d: 下一次的检测方向, simp: 去除离原点较远的点后, 剩余的两个顶点
        """
        a, b, c = self.a, self.b, self.c
        a, b, c = to_np([a, b, c])
        simp = self

        ab = b - a
        ao = -a

        # --- 开始检测
        ret = False
        if simp.__len__() == 3:
            ac = c - a
            ac_normal = get_abc_cross(ab, ac, ac)
            ab_normal = get_abc_cross(ac, ab, ab)

            if ab_normal.dot(ao) > 0:
                simp.remove_by_name('c')
                d = ab_normal
            else:
                if ac_normal.dot(ao) > 0:
                    simp.remove_by_name('c')
                    d = ac_normal
                else:
                    ret = True
                    d = None
        else:
            d = get_d_bycross(a, b)

        self.d = d
        return ret, d, simp


def BoxCheck(aa: ShapeBase, bb: ShapeBase):
    """
    包围盒检测, 即AABB算法
    """
    if abs(aa.position[0] - bb.position[0] < (aa.width + bb.width)) / 2 and abs(aa.position[1] - bb.position[1]) < (aa.height + bb.height)/ 2:
        return True
    else:
        return False


def GJKCheck(aa: ShapeBase, bb: ShapeBase, simp=None):
    """
    判断两个多边形是否发生了碰撞
    """
    if simp is None:
        simp = Simplex()        # 单纯形

    # 根据多边形中心计算出初始方向d, 然后根据d计算第一个点并入栈
    d = aa.position - bb.position
    simp.add(support(aa, bb, d))
    d = -d

    N = aa.shape[0] * bb.shape[0]       # 最大循环次数
    i = 0
    while True:
        i += 1
        if i > N:
            print('!!!!超过最大迭代次数!!!')
            return False

        simp.add(support(aa, bb, d))
        print(f'\n -- {d} ----- simp.a.dot(d):', simp.a.dot(d))
        if simp.a.dot(d) < 0:       # 若搜索方向(即面向原点的方向)和新得到的点夹角小于90°, 说明没发生碰撞.
            return False
        else:
            print('********************************* simp_0: ', simp)
            is_collision, d, simp = simp.ContainsOrigin()
            print(is_collision, d)
            print('~~~ simp_1: ', simp)

            print()
            if is_collision:
                return True


def Check(aa, bb):
    if BoxCheck(aa, bb) and GJKCheck(aa, bb):
        return True
    return False


def ContainsOrigin(a, b, c):
    """
    根据三角形三个点的坐标, 判断其是否包含原点
    :param a: 点a
    :param b: 点b
    :param c: 点c
    :return: ret: 是否碰撞, d: 下一次的检测方向, simp: 去除离原点较远的点后, 剩余的两个顶点
    """
    a, b, c = to_np([a, b, c])
    simp = Simplex([a, b, c])

    ab = b - a
    ao = -a

    # --- 开始检测
    ret = False
    if simp.__len__() == 3:
        ac = c - a
        ac_normal = get_abc_cross(ab, ac, ac)
        ab_normal = get_abc_cross(ac, ab, ab)

        if ab_normal.dot(ao) > 0:
            simp.remove_by_name('c')
            d = ab_normal
        else:
            if ac_normal.dot(ao) > 0:
                simp.remove_by_name('c')
                d = ac_normal
            else:
                ret = True
                d = None
    else:
        d = get_d_bycross(a, b)

    return ret, d, simp


class Edge:
    def __init__(self, a, b=None):
        if b is None:
            b = [0, 0]
        a, b = to_np([a, b])
        self.a = a
        self.b = b
        self.ab = b - a
        self.distance = self._get_distance()
        self.normal = self._get_normal(args.epa_e_norm)

    def _get_distance(self):
        """
        两点式: (y-y1)/(y2-y1) = (x-x1)/(x2-x1)
        化简得l_ab表达式: (y2-y1)*x + (x1-x2)*y + (x2*y1-x1*y2) = 0
        点到直线距离: abs(A*x + B*y + C)/sqrt(A**2 + B**2)

        原点到直线距离ab的为: abs(x2*y1 - x1*y2)/sqrt((y2-y1)**2 + (y1-y2)**2)
        """
        x1, y1 = self.a
        x2, y2 = self.b

        if (y2 - y1) ** 2 + (x1 - x2) ** 2 < args.epsilon:
            # if point [a, b] is overlap, return |a|.
            return np.linalg.norm(self.a)

        if (self.a == self.b).all():
            dist = sqrt(x1 ** 2 + x2 ** 2)  # 一个点的情况
        else:
            dist = abs(x2 * y1 - x1 * y2) / (sqrt((y2 - y1) ** 2 + (x1 - x2) ** 2))  # 防止被除数为0
        return dist

    def _get_normal(self, norm=True):
        """
        得到ab的正交向量, 且反向于原点
        """
        ret = -get_d_bycross(self.a, self.b, norm=norm)
        return ret

    def __repr__(self):
        return f'Edge [{self.a}, {self.b} --- ab: {self.ab}, distance: {self.distance}]'

    def judge_p_in_e(self, p):
        """
        判断点p有没有在e中
        """
        x1, y1 = self.a
        x2, y2 = self.b
        x, y = p

        def x_in_x12(x, x1, x2):
            if min(x1, x2) <= x1 <= max(x1, x2):
                return True
            return False

        if x_in_x12(x, x1, x2) and x_in_x12(y, y1, y2):
            if (y2 - y1) * x + (x1 - x2) * y + (x2 * y1 - x1 * y2) == 0:
                return True

        return False

    def __eq__(self, other):

        if np.equal(self.a, other.a).all() and np.equal(self.b, other.b).all():
            return True
        else:
            return False


class EPAPolygon:
    """
    由最开始的单纯形推导出多边形
    """

    def __init__(self, simp: Simplex):
        self.simp = simp
        ls = simp.ls

        edges = []
        for i in range(len(ls)):
            a = ls[i]
            for j in range(i + 1, len(ls)):
                b = ls[j]
                e = Edge(a, b)
                edges.append(e)
        self.edges = edges

    @property
    def points(self):
        ret = []
        for e in self.edges:
            # e = self.edges[0]
            a = e.a.tolist()
            if a not in ret:
                ret.append(a)

            b = e.b.tolist()
            if b not in ret:
                ret.append(b)
        return ret

    def insert_edge_point(self, e: Edge, point):
        """
        将边e从point位置分割为两条新的边, 然后将旧的e删除

        e = a_b = b - a
        p_a = a - p
        p_b = b - p
        """
        p = to_np(point)

        p_a = Edge(e.a, p)
        p_b = Edge(e.b, p)

        self.edges.remove(e)
        self.edges.append(p_a)
        self.edges.append(p_b)

    def find_closest_edge(self):
        min_dist = float('inf')
        min_i = 0
        for i in range(len(self.edges)):
            e = self.edges[i]
            if e.distance < min_dist:
                min_i = i
                min_dist = e.distance
        ret = self.edges[min_i]
        # ret = self.edges.pop(min_i)
        return ret

    def get_i_by_e(self, e: Edge):
        """
        根据e的值, 查找出对应的index.
        返回id, 若id等于-1为没有找到e.
        """
        for i in range(len(self.edges)):
            ei = self.edges[i]
            if ei == e:
                return i
        return -1

    def info(self):
        self.simp.info()
        print('------ 边:')
        show_ls(self.edges)

    def judge_contains_p(self, p):
        for e in self.edges:
            if e.judge_p_in_e(p):
                return True
        return False

    def judge_contains_e(self, e: Edge):
        """
        判断e是否在polygon中
        """
        for e0 in self.edges:
            if e0 == e:
                return True
        return False

    def __repr__(self):
        return f'多边形边数: {len(self.edges)}'


def EPA_Algorithom(aa: ShapeBase, bb: ShapeBase, simp: Simplex, MAX_RET_LS=2):
    """
    若多边形aa和bb产生了碰撞, 则找出穿透向量
    simp为GJK碰撞检测得出的单纯形

    MAX_RET_LS: 最多返回的穿透向量个数, 默认2个
    - ps: 若想找到多个方向向量, 则叉积get_d_bycross在AB平行OA, 或A点和原点重合时, 需要随机取值.
    """
    poly = EPAPolygon(simp)

    N = aa.shape[0] * bb.shape[0] - len(simp)  # 最大循环次数为 点集数量 - 单纯形顶点数
    i = 0
    # ret_ls = []
    while True:
        i += 1

        # 找到距离原点最近的边
        e = poly.find_closest_edge()
        print('___ e.normal:', e.normal, '____e.distance:', e.distance)

        # 沿着边的法线方向，尝试找一个新的support点
        p = support(aa, bb, e.normal)

        # 无法找到能够跨越该边的support点了, 也就是说，该边就是差集最近边
        new_distance = e.normal.dot(p)
        if new_distance <= e.distance:
            # # 返回一个方向上的穿透向量
            ret = e.normal * e.distance
            break

        print(f'------ p: {p}, new_distance:', round(new_distance, 3), '\n ============', e)

        if e.judge_p_in_e(p):
            print('=========== 新的点在旧边上, 将陷入无限循环.')
            ret = e.normal * e.distance
            break

        if i > N:
            # 遍历后发现第一个就是最大的穿透向量了
            print('========= 遍历后发现第一个就是最大的穿透向量了 =========')
            ret = e.normal * e.distance
            break

        # 将新的support点插入多边形中(将旧的边e分割为新的两条边)
        poly.insert_edge_point(e, p)
        from plot_polygon import plot_polygon, get_sorted_points
        points = get_sorted_points(poly.points)
        plot_polygon(points)

    # return ret_ls
    return ret
